# -*- coding: euc-jp -*-

require 'rubygems'
require 'serialport'
require 'thread'
require 'debugout'

class Servo
  COMPONENT_NAME = :servo

  RETRY_MAX = 1
  FEEDBACK_LOOP_INTERVAL = 0.01  # [second]
  LENGTH_PER_PULSE       = 2.0 / 200  # [mm / one pulse]

  MAX_INV_VELOCITY = 100

  def initialize
  end

  def init_as_component(component_manager)
    @component_manager = component_manager
    @parameter = @component_manager.component(:parameter)
    @port = open_port(@parameter)
    @mutex = Mutex.new

    @command_pos = [0, 0, 0]
    @current_pos = [0, 0, 0]
    @inv_velocity = []
    @feed_rate = 0

    # create feedback loop
    Thread.new do
      begin
        feedback_loop
      rescue
        STDERR.puts "#{$!}"
      end
    end
  end


  def set_command_pos(pos)
    pos.map!{|v| (v / LENGTH_PER_PULSE).to_i}
    @command_pos = pos
    set_feed_rate(@feed_rate)

    cmd = "sg " + pos.join(" ") + ";gg;"

    Debugout.puts Debugout::LEVEL_DEBUG, cmd

    @mutex.synchronize do
      RETRY_MAX.times do
        @port.print(cmd)
        res = @port.gets
        @command_pos = res.split(" ").map{|v| v.to_i}
        break if pos == @command_pos
      end
    end
  end

  def set_feed_rate(feed_rate) # range [0, 1.0]
    @feed_rate = feed_rate

    return if in_position?

    d_vec = []
    @command_pos.each_index{|i| d_vec.push(@command_pos[i] - @current_pos[i])}
    d = 0
    d_vec.each{|v| d += v*v}
    d = Math.sqrt(d)
    speed_vec = d_vec.map{|v| v = (v / d).abs * @feed_rate}
    inv_velocity_vec = speed_vec.map{|v| v > 0 ? (MAX_INV_VELOCITY / v).to_i : 0}
    cmd = "sv " + inv_velocity_vec.join(" ") + ";gv;"

    @mutex.synchronize do
      RETRY_MAX.times do
        Debugout.puts Debugout::LEVEL_DEBUG, cmd
        @port.print(cmd)
        res = @port.gets
        Debugout.puts Debugout::LEVEL_DEBUG, res
        @inv_velocity = res.split(" ").map{|v| v.to_i}
        break if inv_velocity_vec == @inv_velocity
      end
    end
  end

  def get_current_pos
    return @current_pos.map{|v| v * LENGTH_PER_PULSE}
  end

  def get_command_pos
    return @command_pos.map{|v| v * LENGTH_PER_PULSE}
  end

  def in_position?
    return @command_pos == @current_pos
  end

  def get_inv_velocity
    return @inv_velocity
  end

  def get_feed_rate
    return @feed_rate
  end

  def set_current_pos_as_origin
    # TBD
  end

  private
  def feedback_loop
    Debugout.puts Debugout::LEVEL_INFO, "begin feedback loop"
    while true
      @mutex.synchronize do
        @port.print("gc;")
        res = @port.gets
        @current_pos = res.split(" ").map{|v| v.to_i}
        Debugout.puts Debugout::LEVEL_DEBUG_MORE, "pos:" + @current_pos.join(" ")
      end
      sleep FEEDBACK_LOOP_INTERVAL
    end
  end

  def open_port(parameter)
    device_name = parameter.get("servo.device_name")
    device_speed = parameter.get("servo.device_speed").to_i
    Debugout.puts Debugout::LEVEL_INFO, "open serialport #{device_name}, #{device_speed}"
    
    port = SerialPort.new(device_name, device_speed, 8, 1, SerialPort::NONE)
    #port.read_timeout = 100
    #port.write_timeout = 100
    return port
  end
end
